package de.steric;

import org.lwjgl.util.vector.Matrix3f;
import org.lwjgl.util.vector.Matrix4f;
import org.lwjgl.util.vector.Quaternion;
import org.lwjgl.util.vector.Vector3f;
import org.lwjgl.util.vector.Vector4f;

import de.steric.battlemon.GameObject;

public class MathHelper {

	public static Matrix4f getRotation(Quaternion q) {
		// fetch the Quaternion values
		float qw = q.w;
		float qx = q.x;
		float qy = q.y;
		float qz = q.z;

		// get some Multiplicator
		float n = 1.0f / (float) (Math.sqrt(qx * qx + qy * qy + qz * qz + qw
				* qw));
		qx *= n;
		qy *= n;
		qz *= n;
		qw *= n;

		// create Transformation matrix
		Matrix4f TransformationMatrix = new Matrix4f();

		// Fill transformation Matrix
		TransformationMatrix.m00 = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz;
		TransformationMatrix.m01 = 2.0f * qx * qy - 2.0f * qz * qw;
		TransformationMatrix.m02 = 2.0f * qx * qz + 2.0f * qy * qw;
		TransformationMatrix.m03 = 0.0f;
		TransformationMatrix.m10 = 2.0f * qx * qy + 2.0f * qz * qw;
		TransformationMatrix.m11 = 1.0f - 2.0f * qx * qx - 2.0f * qz * qz;
		TransformationMatrix.m12 = 2.0f * qy * qz - 2.0f * qx * qw;
		TransformationMatrix.m13 = 0.0f;
		TransformationMatrix.m20 = 2.0f * qx * qz - 2.0f * qy * qw;
		TransformationMatrix.m21 = 2.0f * qy * qz + 2.0f * qx * qw;
		TransformationMatrix.m22 = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy;
		TransformationMatrix.m23 = 0.0f;
		TransformationMatrix.m30 = 0.0f;
		TransformationMatrix.m31 = 0.0f;
		TransformationMatrix.m32 = 0.0f;
		TransformationMatrix.m33 = 1.0f;
		return TransformationMatrix;
	}

	public static Vector4f extendPosition(Vector3f input) {
		return new Vector4f(input.x, input.y, input.z, 1);
	}

	public static Vector4f extendDirection(Vector3f input) {
		return new Vector4f(input.x, input.y, input.z, 0);
	}

	public static Vector3f shrink(Vector4f input) {
		return new Vector3f(input.x, input.y, input.z);
	}
	public static Matrix3f shrink(Matrix4f input)
	{
		Matrix3f res= new Matrix3f();
		res.m00=input.m00;
		res.m01=input.m01;
		res.m02=input.m02;
		res.m10=input.m10;
		res.m11=input.m11;
		res.m12=input.m12;
		res.m20=input.m20;
		res.m21=input.m21;
		res.m22=input.m22;
		return res;
	}
	public static float Distance(GameObject a, GameObject b) {
		Vector3f distVector = new Vector3f(a.getPosition().x
				- b.getPosition().x, a.getPosition().y - b.getPosition().y,
				a.getPosition().z - b.getPosition().z);
		return distVector.length();
	}

	/**
	 * Converts degrees to radians.
	 * 
	 * @param degrees
	 *            - input degrees
	 * @return - radians
	 */
	public static double d2R(double degrees) {
		return degrees * Math.PI / 180d;
	}

	/**
	 * Converts radians to degrees.
	 * 
	 * @param radians
	 *            - input radians
	 * @return - degrees
	 */
	public static double r2D(double radians) {
		return radians * 180d / Math.PI;
	}
	/**
	 * Create a projectionMatrix 
	 * @param fieldOfView - the fov in degrees
	 * @param aspectRatio - with/height
	 * @param nearPlane - mostly 0.1f
	 * @param farPlane - mostly 100f
	 * @return - the desired projectionMatrix
	 */
	public static Matrix4f getProjectionMatrix(float fieldOfView,
			float aspectRatio, float nearPlane, float farPlane) {
		Matrix4f res = new Matrix4f();
		float y_scale = 1 / (float) (Math.tan(d2R(fieldOfView) / 2));
		float x_scale = y_scale / aspectRatio;
		float frustrum_length = farPlane - nearPlane;
		res.m00 = x_scale;
		res.m11 = y_scale;
		res.m22 = -((farPlane + nearPlane) / frustrum_length);
		res.m23 = -1;
		res.m32 = -((2 * nearPlane * farPlane) / frustrum_length);
		res.m33 = 0;
		return res;
	}
}
